
#include "chassis_protocol.h"


uint8_t can_msg_check_sum(unsigned int id, uint8_t * data, uint8_t len)
{
    uint8_t checksum = 0x00;
    checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + len;

    for(uint8_t i = 0; i < (len - 1); i++)
    {
        checksum += data[i];
    }

    return checksum;
}
/*0x130: motion control filed*/
/*
byte0: control_mode
byte1: clear error cmd
byte2: linear_velocity_percentage  xx% of 1.5m/s {-100~100}
byte3: angular_velocity_percentage xx% of 0.7853 rad/s{-100~100}
byte4: RSV
byte5: RSV
byte6: count
byte7: check_sum
*/
rosCtlChassisSpeed_t ros_decode_msg(userCanBuf_t *canBuf)
{
    rosCtlChassisSpeed_t rosCtlSpe;
    int8_t linear_x = 0,angle_z= 0;

    if(canBuf->can_data[7] == can_msg_check_sum(canBuf->ID,canBuf->can_data,8))
    {
        linear_x = canBuf->can_data[2];
        angle_z = canBuf->can_data[3];
    }
//    SEGGER_RTT_printf(0," linear_x: angle :%d  %d \n",linear_x,angle_z);
    rosCtlSpe.linear_velocity = (float)linear_x*MAX_LIN_VELOCITY/100;
    rosCtlSpe.angle_velocity = (float)angle_z*MAX_ANG_VELOCITY/100;
    return rosCtlSpe;
}

chassis_raw_speed_t chassis_decode_msg(userCanBuf_t *recvBuf)
{
    static chassis_raw_speed_t chassis_raw_speed;

    switch(recvBuf->ID)
    {
    case FRONT_LEFT_R_ID:
        chassis_raw_speed.front_left = recvBuf->can_data[0]<<24 \
                                       | recvBuf->can_data[1]<<16 \
                                       | recvBuf->can_data[2]<<8 \
                                       | recvBuf->can_data[3];
        break;

    case FRONT_RIGHT_R_ID:
        chassis_raw_speed.front_right = recvBuf->can_data[0]<<24 \
                                        | recvBuf->can_data[1]<<16 \
                                        | recvBuf->can_data[2]<<8 \
                                        | recvBuf->can_data[3];
        break;

    case REAR_LEFT_R_ID:
        chassis_raw_speed.rear_left = recvBuf->can_data[0]<<24 \
                                      | recvBuf->can_data[1]<<16 \
                                      | recvBuf->can_data[2]<<8 \
                                      | recvBuf->can_data[3];
        break;

    case REAR_RIGHT_R_ID:
        chassis_raw_speed.rear_right = recvBuf->can_data[0]<<24 \
                                       | recvBuf->can_data[1]<<16 \
                                       | recvBuf->can_data[2]<<8 \
                                       | recvBuf->can_data[3];
        break;
    }

    return chassis_raw_speed;
}


chassis_raw_speed_t calc_raw_speed(float linear_velocity,float angle_velocity)
{
    float diff_speeed = 0;
    float front_left,front_right;
    chassis_raw_speed_t rawSpeed;
    //diff
    diff_speeed = WHEEL_BASE * angle_velocity;
    front_left = linear_velocity + diff_speeed;
    front_right = linear_velocity - diff_speeed;
    //calc speed
    rawSpeed.front_left = (int)(-front_left * VEL_PARAM);
    rawSpeed.rear_left = rawSpeed.front_left;
    rawSpeed.front_right = (int)(front_right * VEL_PARAM);
    rawSpeed.rear_right = rawSpeed.front_right;
    return rawSpeed;
}

bool send_front_left_speed(chassis_raw_speed_t speed)
{
    userCanBuf_t canSendbuff;
    canSendbuff.ID = FRONT_LEFT_S_ID;
    canSendbuff.can_data[0] = (speed.front_left >> 24)& 0xff;
    canSendbuff.can_data[1] = (speed.front_left >> 16)& 0xff;
    canSendbuff.can_data[2] = (speed.front_left >> 8)& 0xff;
    canSendbuff.can_data[3] = (speed.front_left >> 0)& 0xff;
    canSendbuff.can_data[4] = 0x01;
    canSendbuff.can_data[5] = 0x10;
    return chasis_vel_ctl(&canSendbuff);
}
bool send_front_right_speed(chassis_raw_speed_t speed)
{
    userCanBuf_t canSendbuff;
    canSendbuff.ID = FRONT_RIGHT_S_ID;
    canSendbuff.can_data[0] = (speed.front_right >> 24)& 0xff;
    canSendbuff.can_data[1] = (speed.front_right >> 16)& 0xff;
    canSendbuff.can_data[2] = (speed.front_right >> 8)& 0xff;
    canSendbuff.can_data[3] = (speed.front_right >> 0)& 0xff;
    canSendbuff.can_data[4] = 0x01;
    canSendbuff.can_data[5] = 0x10;
    return chasis_vel_ctl(&canSendbuff);
}

bool send_rear_left_speed(chassis_raw_speed_t speed)
{
    userCanBuf_t canSendbuff;
    canSendbuff.ID = REAR_LEFT_S_ID;
    canSendbuff.can_data[0] = (speed.rear_left >> 24)& 0xff;
    canSendbuff.can_data[1] = (speed.rear_left >> 16)& 0xff;
    canSendbuff.can_data[2] = (speed.rear_left >> 8)& 0xff;
    canSendbuff.can_data[3] = (speed.rear_left >> 0)& 0xff;
    canSendbuff.can_data[4] = 0x01;
    canSendbuff.can_data[5] = 0x10;
    return chasis_vel_ctl(&canSendbuff);
}
bool send_rear_right_speed(chassis_raw_speed_t speed)
{
    userCanBuf_t canSendbuff;
    canSendbuff.ID = REAR_RIGHT_S_ID;
    canSendbuff.can_data[0] = (speed.rear_right >> 24)& 0xff;
    canSendbuff.can_data[1] = (speed.rear_right >> 16)& 0xff;
    canSendbuff.can_data[2] = (speed.rear_right >> 8)& 0xff;
    canSendbuff.can_data[3] = (speed.rear_right >> 0)& 0xff;
    canSendbuff.can_data[4] = 0x01;
    canSendbuff.can_data[5] = 0x10;
    return chasis_vel_ctl(&canSendbuff);
}

void chassis_status_package(userCanBuf_t* can_buf,short linear_vel,short angular_vel)
{
  static uint8_t send_cnt = 0;
  can_buf->ID = CAN_ID_MOTION_STATUS;
  can_buf->can_data[0] = (uint8_t)(linear_vel >> 8);
  can_buf->can_data[1] = (uint8_t)(linear_vel);
  can_buf->can_data[2] = (uint8_t)(angular_vel >> 8);
  can_buf->can_data[3] = (uint8_t)(angular_vel);
  can_buf->can_data[4] = send_cnt ++;
  can_buf->can_data[5] = 0;
  can_buf->can_data[6] = 0;
  can_buf->can_data[7] = 0;
}



/************** END OF FILE ******************/



